control.h File Reference

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	ControlSubsystem

Typedefs
typedef struct ControlSubsystem	ControlSubsystem
Control Port Function Type Definitions
The "write" and "stream" commands provide two control functions visible at the main() level. These typedefs define the structure of those two calls.
typedef int8_t(	writePort_t) (struct ControlSubsystem *pComm, uint8_t buffer[], uint16_t nbytes)
typedef void(	streamData_t) (SensorFusionGlobals *sfg, uint8_t *sUARTOutputBuffer)

Functions
int8_t	initializeControlPort (ControlSubsystem *pComm)
void	CreateAndSendPackets (SensorFusionGlobals *sfg, uint8_t *sUARTOutputBuffer)
void	DecodeCommandBytes (SensorFusionGlobals *sfg, char iCommandBuffer[], uint8 sUART_InputBuffer[], uint16 nbytes)
void	BlueRadios_Init (void)
void	sBufAppendItem (uint8_t *pDest, uint16_t *pIndex, uint8_t *pSource, uint16_t iBytesToCopy)

Variables
uint8_t	sUARTOutputBuffer [256]

Detailed Description

Each sensor fusion application will probably have its own set of functions to control the fusion process and report results. This file defines the programming interface that should be followed in order for the fusion functions to operate correctly out of the box. The actual command interpreter is defined separately in DecodeCommandBytes.c. The output streaming function is defined in output_stream.c. Via these three files, the NXP Sensor Fusion Library provides a default set of functions which are compatible with the Sensor Fusion Toolbox. Use of the toolbox is highly recommended at least during initial development, as it provides many useful debug features. The NXP development team will typically require use of the toolbox as a pre-requisite for providing software support.

Definition in file control.h.

Typedef Documentation

typedef struct ControlSubsystem ControlSubsystem

he ControlSubsystem encapsulates command and data streaming functions.

The ControlSubsystem encapsulates command and data streaming functions for the library. A C++-like typedef structure which includes executable methods for the subsystem is defined here.

typedef void( streamData_t) (SensorFusionGlobals *sfg, uint8_t *sUARTOutputBuffer)

Definition at line 56 of file control.h.

typedef int8_t( writePort_t) (struct ControlSubsystem *pComm, uint8_t buffer[], uint16_t nbytes)

Definition at line 55 of file control.h.

Function Documentation

void BlueRadios_Init

(

void

)

Used to initialize the Blue Radios Bluetooth module found on the FRDM-FXS-MULT2-B sensor shield from NXP.

Definition at line 132 of file control.c.

Referenced by initializeControlPort().

{
    uint16_t ilen;      // command string length

    // transmit "ATSRM,2,0\r" to minimize traffic from the module
    // command "ATSRM": sets the module response mode which configures how verbose the module will be
    // 2: response mode at to minimal
    // 0: disconnected mode is command mode
    // \r: carriage return escape sequence
    strcpy((char *)sUARTOutputBuffer, "ATSRM,2,0\r");
    ilen = strlen((char *)sUARTOutputBuffer);
        writeWirelessPort(sUARTOutputBuffer, ilen);
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void CreateAndSendPackets	(	SensorFusionGlobals *	sfg,
		uint8_t *	sUARTOutputBuffer
	)

Called once per fusion cycle to stream information required by the NXP Sensor Fusion Toolbox. Packet protocols are defined in the NXP Sensor Fusion for Kinetis Product Development Kit User Guide.

Definition at line 144 of file output_stream.c.

Referenced by initializeControlPort().

{
    Quaternion      fq;                 // quaternion to be transmitted
    float           ftmp;               // scratch
    static uint32_t iTimeStamp = 0;     // 1MHz time stamp
    uint16_t        iIndex;             // output buffer counter
    int32_t         scratch32;          // scratch int32_t
    int16_t         scratch16;          // scratch int16_t
    int16_t         iPhi,
                    iThe,
                    iRho;               // integer angles to be transmitted
    int16_t         iDelta;             // magnetic inclination angle if available
    int16_t         iOmega[3];          // scaled angular velocity vector
    uint16_t        isystick;           // algorithm systick time
    int16_t         i, j, k;            // general purpose
    uint8_t         tmpuint8_t;         // scratch uint8_t
    uint8_t         flags;              // byte of flags
    uint8_t         AngularVelocityPacketOn,
                    DebugPacketOn,
                    RPCPacketOn;
    int8_t          AccelCalPacketOn;
    static uint8_t  iPacketNumber = 0;  // packet number

    // update the 1MHz time stamp counter expected by the PC GUI (independent of project clock rates)
    iTimeStamp += 1000000 / FUSION_HZ;

#if (MAXPACKETRATEHZ < FUSION_HZ)
    uint8_t  skip_packet = throttle(); // possible UART bandwidth problem
    if (skip_packet) return;  // need to skip packet transmission to avoid UART overrun
#endif

    // cache local copies of control flags so we don't have to keep dereferencing pointers below
    quaternion_type quaternionPacketType;
    quaternionPacketType = sfg->pControlSubsystem->QuaternionPacketType;
    AngularVelocityPacketOn = sfg->pControlSubsystem->AngularVelocityPacketOn;
    DebugPacketOn = sfg->pControlSubsystem->DebugPacketOn;
    RPCPacketOn = sfg->pControlSubsystem->RPCPacketOn;
    AccelCalPacketOn = sfg->pControlSubsystem->AccelCalPacketOn;

    // zero the counter for bytes accumulated into the transmit buffer
    iIndex = 0;

    // ************************************************************************
    // Main type 1: range 0 to 35 = 36 bytes
    // Debug type 2: range 0 to 7 = 8 bytes
    // Angular velocity type 3: range 0 to 13 = 14 bytes
    // Euler angles type 4: range 0 to 13 = 14 bytes
    // Altitude/Temp type 5: range 0 to 13 = 14 bytes
    // Magnetic type 6: range 0 to 16 = 18 bytes
    // Kalman packet 7: range 0 to 47 = 48 bytes
    // Precision Accelerometer packet 8: range 0 to 46 = 47 bytes
    //
    // Total excluding intermittent packet 8 is:
    // 152 bytes vs 256 bytes size of sUARTOutputBuffer
    // at 25Hz, data rate is 25*152 = 3800 bytes/sec = 38.0kbaud = 33% of 115.2kbaud
    // at 40Hz, data rate is 40*152 = 6080 bytes/sec = 60.8kbaud = 53% of 115.2kbaud
    // at 50Hz, data rate is 50*152 = 7600 bytes/sec = 76.0kbaud = 66% of 115.2kbaud
    // ************************************************************************
    // ************************************************************************
    // fixed length packet type 1
    // this packet type is always transmitted
    // total size is 0 to 35 equals 36 bytes
    // ************************************************************************
    // [0]: packet start byte (need a iIndex++ here since not using sBufAppendItem)
    sUARTOutputBuffer[iIndex++] = 0x7E;

    // [1]: packet type 1 byte (iIndex is automatically updated in sBufAppendItem)
    tmpuint8_t = 0x01;
    sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

    // [2]: packet number byte
    sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
    iPacketNumber++;

    // [6-3]: 1MHz time stamp (4 bytes)
    sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iTimeStamp, 4);

    // [12-7]: integer accelerometer data words (scaled to 8192 counts per g for PC GUI)
    // send non-zero data only if the accelerometer sensor is enabled and used by the selected quaternion
    if (sfg->iFlags & F_USING_ACCEL) {
        switch (quaternionPacketType)
        {
            case Q3:
            case Q6MA:
            case Q6AG:
            case Q9:
#if F_USING_ACCEL
                // accelerometer data is used for the selected quaternion so transmit but clip at 4g
                scratch32 = (sfg->Accel.iGc[CHX] * 8192) / sfg->Accel.iCountsPerg;
                if (scratch32 > 32767) scratch32 = 32767;
                if (scratch32 < -32768) scratch32 = -32768;
                scratch16 = (int16_t) (scratch32);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

                scratch32 = (sfg->Accel.iGc[CHY] * 8192) / sfg->Accel.iCountsPerg;
                if (scratch32 > 32767) scratch32 = 32767;
                if (scratch32 < -32768) scratch32 = -32768;
                scratch16 = (int16_t) (scratch32);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

                scratch32 = (sfg->Accel.iGc[CHZ] * 8192) / sfg->Accel.iCountsPerg;
                if (scratch32 > 32767) scratch32 = 32767;
                if (scratch32 < -32768) scratch32 = -32768;
                scratch16 = (int16_t) (scratch32);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;
#endif // F_USING_ACCEL
            case Q3M:
            case Q3G:
            default:
                // accelerometer data is not used in currently selected algorithm so transmit zero
                sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
                break;
        }
     } else {
                // accelerometer structure is not defined so transmit zero
                sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
        }
    // [18-13]: integer calibrated magnetometer data words (already scaled to 10 count per uT for PC GUI)
    // send non-zero data only if the magnetometer sensor is enabled and used by the selected quaternion
    if (sfg->iFlags & F_USING_MAG)
        switch (quaternionPacketType)
        {
            case Q3M:
            case Q6MA:
            case Q9:
#if F_USING_MAG
                // magnetometer data is used for the selected quaternion so transmit
                scratch16 = (int16_t) (sfg->Mag.iBc[CHX] * 10) / (sfg->Mag.iCountsPeruT);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) ((sfg->Mag.iBc[CHY] * 10) / sfg->Mag.iCountsPeruT);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) ((sfg->Mag.iBc[CHZ] * 10) / sfg->Mag.iCountsPeruT);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;
#endif
            // magnetometer data is not used in currently selected algorithm so transmit zero
            case Q3:
            case Q3G:
            case Q6AG:
            default:
                sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
                break;
        }
    else
    {
        // magnetometer structure is not defined so transmit zero
        sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
    }

    // [24-19]: uncalibrated gyro data words (scaled to 20 counts per deg/s for PC GUI)
    // send non-zero data only if the gyro sensor is enabled and used by the selected quaternion
    if (sfg->iFlags & F_USING_GYRO)
    {
        switch (quaternionPacketType)
        {
            case Q3G:
            case Q6AG:
#if F_USING_GYRO
        case Q9:

              // gyro data is used for the selected quaternion so transmit
                scratch16 = (int16_t) ((sfg->Gyro.iYs[CHX] * 20) / sfg->Gyro.iCountsPerDegPerSec);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) ((sfg->Gyro.iYs[CHY] * 20) / sfg->Gyro.iCountsPerDegPerSec);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) ((sfg->Gyro.iYs[CHZ] * 20) / sfg->Gyro.iCountsPerDegPerSec);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;
#endif
            case Q3:
            case Q3M:
            case Q6MA:
            default:
                // gyro data is not used in currently selected algorithm so transmit zero
                sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
                break;
        }
    }
    else
    {
        // gyro structure is not defined so transmit zero
        sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
    }

    // initialize default quaternion, flags byte, angular velocity and orientation
    fq.q0 = 1.0F;
    fq.q1 = fq.q2 = fq.q3 = 0.0F;
    flags = 0x00;
    iOmega[CHX] = iOmega[CHY] = iOmega[CHZ] = 0;
    iPhi = iThe = iRho = iDelta = 0;
    isystick = 0;

    // flags byte 33: quaternion type in least significant nibble
    // Q3:   coordinate nibble, 1
    // Q3M:  coordinate nibble, 6
    // Q3G:  coordinate nibble, 3
    // Q6MA: coordinate nibble, 2
    // Q6AG: coordinate nibble, 4
    // Q9:   coordinate nibble, 8
    // flags byte 33: coordinate in most significant nibble
    // Aerospace/NED:   0, quaternion nibble
    // Android:         1, quaternion nibble
    // Windows 8:       2, quaternion nibble
    // set the quaternion, flags, angular velocity and Euler angles
    switch (quaternionPacketType)
    {
#if F_3DOF_G_BASIC
        case Q3:
            if (sfg->iFlags & F_3DOF_G_BASIC)
            {
                flags |= 0x01;
                readCommon((SV_ptr)&sfg->SV_3DOF_G_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
            }
            break;
#endif
#if F_3DOF_B_BASIC
        case Q3M:
            if (sfg->iFlags & F_3DOF_B_BASIC)
            {
                flags |= 0x06;
                readCommon((SV_ptr)&sfg->SV_3DOF_B_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
            }
            break;
#endif
#if F_3DOF_Y_BASIC
        case Q3G:
            if (sfg->iFlags & F_3DOF_Y_BASIC)
            {
                flags |= 0x03;
                readCommon((SV_ptr)&sfg->SV_3DOF_Y_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
            }
            break;
#endif
#if F_6DOF_GB_BASIC
        case Q6MA:
            if (sfg->iFlags & F_6DOF_GB_BASIC)
            {
                flags |= 0x02;
                iDelta = (int16_t) (10.0F * sfg->SV_6DOF_GB_BASIC.fLPDelta);
                readCommon((SV_ptr)&sfg->SV_6DOF_GB_BASIC, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
            }
            break;
#endif
#if F_6DOF_GY_KALMAN
        case Q6AG:
            if (sfg->iFlags & F_6DOF_GY_KALMAN)
            {
                flags |= 0x04;
                readCommon((SV_ptr)&sfg->SV_6DOF_GY_KALMAN, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
            }
            break;
#endif
#if F_9DOF_GBY_KALMAN
        case Q9:
            if (sfg->iFlags & F_9DOF_GBY_KALMAN)
             {
                flags |= 0x08;
                iDelta = (int16_t) (10.0F * sfg->SV_9DOF_GBY_KALMAN.fDeltaPl);
                readCommon((SV_ptr)&sfg->SV_9DOF_GBY_KALMAN, &fq, &iPhi, &iThe, &iRho, iOmega, &isystick);
            }
            break;
#endif
        default:
            // use the default data already initialized
            break;
    }

    // [32-25]: scale the quaternion (30K = 1.0F) and add to the buffer
    scratch16 = (int16_t) (fq.q0 * 30000.0F);
    sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
    scratch16 = (int16_t) (fq.q1 * 30000.0F);
    sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
    scratch16 = (int16_t) (fq.q2 * 30000.0F);
    sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
    scratch16 = (int16_t) (fq.q3 * 30000.0F);
    sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

    // set the coordinate system bits in flags from default NED (00)
#if THISCOORDSYSTEM == ANDROID
    // set the Android flag bits
    flags |= 0x10;
#elif THISCOORDSYSTEM == WIN8
    // set the Win8 flag bits
    flags |= 0x20;
#endif // THISCOORDSYSTEM

    // [33]: add the flags byte to the buffer
    sBufAppendItem(sUARTOutputBuffer, &iIndex, &flags, 1);

    // [34]: add the shield (bits 7-5) and Kinetis (bits 4-0) byte
    tmpuint8_t = ((THIS_SHIELD & 0x07) << 5) | (THIS_BOARD & 0x1F);
    sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

    // [35]: add the tail byte for the standard packet type 1
    sUARTOutputBuffer[iIndex++] = 0x7E;

    // ************************************************************************
    // Variable length debug packet type 2
    // total size is 0 to 7 equals 8 bytes
    // ************************************************************************
    if (DebugPacketOn)
    {
        // [0]: packet start byte
        sUARTOutputBuffer[iIndex++] = 0x7E;

        // [1]: packet type 2 byte
        tmpuint8_t = 0x02;
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

        // [2]: packet number byte
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
        iPacketNumber++;

        // [4-3] software version number
        scratch16 = THISBUILD;
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [6-5] systick count / 20
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &isystick, 2);

        // [7 in practice but can be variable]: add the tail byte for the debug packet type 2
        sUARTOutputBuffer[iIndex++] = 0x7E;
    }

    // ************************************************************************
    // Angular Velocity packet type 3
    // total bytes for packet type 2 is range 0 to 13 = 14 bytes
    // ************************************************************************
    if (AngularVelocityPacketOn)
    {
        // [0]: packet start byte
        sUARTOutputBuffer[iIndex++] = 0x7E;

        // [1]: packet type 3 byte (angular velocity)
        tmpuint8_t = 0x03;
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

        // [2]: packet number byte
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
        iPacketNumber++;

        // [6-3]: time stamp (4 bytes)
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iTimeStamp, 4);

        // [12-7]: add the scaled angular velocity vector to the output buffer
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iOmega[CHX], 2);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iOmega[CHY], 2);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iOmega[CHZ], 2);

        // [13]: add the tail byte for the angular velocity packet type 3
        sUARTOutputBuffer[iIndex++] = 0x7E;
    }

    // ************************************************************************
    // Roll, Pitch, Compass Euler angles packet type 4
    // total bytes for packet type 4 is range 0 to 13 = 14 bytes
    // ************************************************************************
    if (RPCPacketOn)
    {
        // [0]: packet start byte
        sUARTOutputBuffer[iIndex++] = 0x7E;

        // [1]: packet type 4 byte (Euler angles)
        tmpuint8_t = 0x04;
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

        // [2]: packet number byte
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
        iPacketNumber++;

        // [6-3]: time stamp (4 bytes)
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iTimeStamp, 4);

        // [12-7]: add the angles (resolution 0.1 deg per count) to the transmit buffer
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iPhi, 2);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iThe, 2);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iRho, 2);

        // [13]: add the tail byte for the roll, pitch, compass angle packet type 4
        sUARTOutputBuffer[iIndex++] = 0x7E;
    }

    // ************************************************************************
    // Altitude / Temperature packet type 5
    // total bytes for packet type 5 is range 0 to 13 = 14 bytes
    // ************************************************************************
#if F_USING_PRESSURE
    if (sfg->iFlags & F_1DOF_P_BASIC)
    {
        if (sfg->pControlSubsystem->AltPacketOn && sfg->Pressure.iWhoAmI)
        {
            // [0]: packet start byte
            sUARTOutputBuffer[iIndex++] = 0x7E;

            // [1]: packet type 5 byte
            tmpuint8_t = 0x05;
            sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

            // [2]: packet number byte
            sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
            iPacketNumber++;

            // [6-3]: time stamp (4 bytes)
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &iTimeStamp,
                           4);

            // [10-7]: altitude (4 bytes, metres times 1000)
            scratch32 = (int32_t) (sfg->SV_1DOF_P_BASIC.fLPH * 1000.0F);
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch32, 4);

            // [12-11]: temperature (2 bytes, deg C times 100)
            scratch16 = (int16_t) (sfg->SV_1DOF_P_BASIC.fLPT * 100.0F);
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

            // [13]: add the tail byte for the altitude / temperature packet type 5
            sUARTOutputBuffer[iIndex++] = 0x7E;
        }
    }
#endif

    // ************************************************************************
    // magnetic buffer packet type 6
    // currently total size is 0 to 17 equals 18 bytes
    // this packet is only transmitted if a magnetic algorithm is computed
    // ************************************************************************
#if F_USING_MAG
    static int16_t  MagneticPacketID = 0;   // magnetic packet number
    if (sfg->iFlags & F_USING_MAG)
    {
        // [0]: packet start byte
        sUARTOutputBuffer[iIndex++] = 0x7E;

        // [1]: packet type 6 byte
        tmpuint8_t = 0x06;
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

        // [2]: packet number byte
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
        iPacketNumber++;

        // [4-3]: number of active measurements in the magnetic buffer
        sBufAppendItem(sUARTOutputBuffer, &iIndex,
                       (uint8_t *) &(sfg->MagBuffer.iMagBufferCount), 2);

        // [6-5]: fit error (%) with resolution 0.01%
        if (sfg->MagCal.fFitErrorpc > 327.67F)
            scratch16 = 32767;
        else
            scratch16 = (int16_t) (sfg->MagCal.fFitErrorpc * 100.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [8-7]: geomagnetic field strength with resolution 0.1uT
        scratch16 = (int16_t) (sfg->MagCal.fB * 10.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // always calculate magnetic buffer row and column (low overhead and saves warnings)
        k = MagneticPacketID - 10;
        j = k / MAGBUFFSIZEX;
        i = k - j * MAGBUFFSIZEX;

        // [10-9]: int16_t: ID of magnetic variable to be transmitted
        // ID 0 to 4 inclusive are magnetic calibration coefficients
        // ID 5 to 9 inclusive are for future expansion
        // ID 10 to (MAGBUFFSIZEX=12) * (MAGBUFFSIZEY=24)-1 or 10 to 10+288-1 are magnetic buffer elements
        // where the convention is used that a negative value indicates empty buffer element (index=-1)
        if ((MagneticPacketID >= 10) && (sfg->MagBuffer.index[i][j] == -1))
        {
            // use negative ID to indicate inactive magnetic buffer element
            scratch16 = -MagneticPacketID;
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        }
        else
        {
            // use positive ID unchanged for variable or active magnetic buffer entry
            scratch16 = MagneticPacketID;
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        }

        // [12-11]: int16_t: variable 1 to be transmitted this iteration
        // [14-13]: int16_t: variable 2 to be transmitted this iteration
        // [16-15]: int16_t: variable 3 to be transmitted this iteration
        switch (MagneticPacketID)
        {
            case 0:
                // item 1: currently unused
                scratch16 = 0;
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

                // item 2: currently unused
                scratch16 = 0;
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

                // item 3: magnetic inclination angle with resolution 0.1 deg
                scratch16 = iDelta;
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;

            case 1:
                // items 1 to 3: hard iron components range -3276uT to +3276uT encoded with 0.1uT resolution
                scratch16 = (int16_t) (sfg->MagCal.fV[CHX] * 10.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) (sfg->MagCal.fV[CHY] * 10.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) (sfg->MagCal.fV[CHZ] * 10.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;

            case 2:
                // items 1 to 3: diagonal soft iron range -32. to +32. encoded with 0.001 resolution
                scratch16 = (int16_t) (sfg->MagCal.finvW[CHX][CHX] * 1000.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) (sfg->MagCal.finvW[CHY][CHY] * 1000.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) (sfg->MagCal.finvW[CHZ][CHZ] * 1000.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;

            case 3:
                // items 1 to 3: off-diagonal soft iron range -32. to +32. encoded with 0.001 resolution
                scratch16 = (int16_t) (sfg->MagCal.finvW[CHX][CHY] * 1000.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) (sfg->MagCal.finvW[CHX][CHZ] * 1000.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                scratch16 = (int16_t) (sfg->MagCal.finvW[CHY][CHZ] * 1000.0F);
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
                break;

            case 4:
            case 5:
            case 6:
            case 7:
            case 8:
            case 9:
                // cases 4 to 9 inclusive are for future expansion so transmit zeroes for now
                sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
                break;

            default:
                // 10 and upwards: this handles the magnetic buffer elements
                sBufAppendItem(sUARTOutputBuffer, &iIndex,
                               (uint8_t *) &(sfg->MagBuffer.iBs[CHX][i][j]), 2);
                sBufAppendItem(sUARTOutputBuffer, &iIndex,
                               (uint8_t *) &(sfg->MagBuffer.iBs[CHY][i][j]), 2);
                sBufAppendItem(sUARTOutputBuffer, &iIndex,
                               (uint8_t *) &(sfg->MagBuffer.iBs[CHZ][i][j]), 2);
                break;
        }

        // wrap the variable ID back to zero if necessary
        MagneticPacketID++;
        if (MagneticPacketID >= (10 + MAGBUFFSIZEX * MAGBUFFSIZEY))
            MagneticPacketID = 0;

        // [17]: add the tail byte for the magnetic packet type 6
        sUARTOutputBuffer[iIndex++] = 0x7E;
    }
#endif

    // *******************************************************************************
    // Kalman filter packet type 7
    // total bytes for packet type 7 is range 0 to 41 inclusive = 42 bytes
    // this packet is only transmitted when a Kalman algorithm is computed
    // and then non-zero data is transmitted only when a Kalman quaternion is selected
    // *******************************************************************************
    bool kalman = false;
#if F_6DOF_GY_KALMAN
    uint8_t six_axis_kalman = (sfg->iFlags & F_6DOF_GY_KALMAN) && (quaternionPacketType == Q6AG);
    kalman = six_axis_kalman;
#endif
#if F_9DOF_GBY_KALMAN
    uint8_t nine_axis_kalman = (sfg->iFlags & F_9DOF_GBY_KALMAN) && (quaternionPacketType == Q9);
    kalman = kalman | nine_axis_kalman;
#endif
#if F_6DOF_GY_KALMAN || F_9DOF_GBY_KALMAN
    if (kalman)
    {
        if ((quaternionPacketType == Q6AG) || (quaternionPacketType == Q9))
        {
            // [0]: packet start byte
            sUARTOutputBuffer[iIndex++] = 0x7E;

            // [1]: packet type 7 byte
            tmpuint8_t = 0x07;
            sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

            // [2]: packet number byte
            sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
            iPacketNumber++;

            // [4-3]: fzgErr[CHX] resolution scaled by 30000
            // [6-5]: fzgErr[CHY] resolution scaled by 30000
            // [8-7]: fzgErr[CHZ] resolution scaled by 30000
            for (i = CHX; i <= CHZ; i++)
            {
#if F_6DOF_GY_KALMAN
                if (six_axis_kalman)    scratch16 = (int16_t) (sfg->SV_6DOF_GY_KALMAN.fZErr[i] * 30000.0F);
#endif
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fZErr[i] * 30000.0F);
#endif
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16,2);
            }

            // [10-9]: fgErrPl[CHX] resolution scaled by 30000
            // [12-11]: fgErrPl[CHY] resolution scaled by 30000
            // [14-13]: fgErrPl[CHZ] resolution scaled by 30000
            for (i = CHX; i <= CHZ; i++)
            {
#if F_6DOF_GY_KALMAN
                if (six_axis_kalman)    scratch16 = (int16_t) (sfg->SV_6DOF_GY_KALMAN.fqgErrPl[i] * 30000.0F);
#endif
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fqgErrPl[i] * 30000.0F);
#endif
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16,2);
            }

            // [16-15]: fzmErr[CHX] resolution scaled by 30000
            // [18-17]: fzmErr[CHY] resolution scaled by 30000
            // [20-19]: fzmErr[CHZ] resolution scaled by 30000
            for (i = CHX; i <= CHZ; i++)
            {
#if F_6DOF_GY_KALMAN
                if (six_axis_kalman)    scratch16 = 0;
#endif
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fZErr[i + 3] * 30000.0F);
#endif
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            }

            // [22-21]: fmErrPl[CHX] resolution scaled by 30000
            // [24-23]: fmErrPl[CHY] resolution scaled by 30000
            // [26-25]: fmErrPl[CHZ] resolution scaled by 30000
            for (i = CHX; i <= CHZ; i++)
            {
#if F_6DOF_GY_KALMAN
                if (six_axis_kalman)    scratch16 = 0;
#endif
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fqmErrPl[i] * 30000.0F);
#endif
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            }

            // [28-27]: fbPl[CHX] resolution 0.001 deg/sec
            // [30-29]: fbPl[CHY] resolution 0.001 deg/sec
            // [32-31]: fbPl[CHZ] resolution 0.001 deg/sec
            for (i = CHX; i <= CHZ; i++)
            {
#if F_6DOF_GY_KALMAN
                if (six_axis_kalman)    scratch16 = (int16_t) (sfg->SV_6DOF_GY_KALMAN.fbPl[i] * 1000.0F);
#endif
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fbPl[i] * 1000.0F);
#endif
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            }

            // [34-33]: fDeltaPl resolution 0.01deg
            scratch16 = 0;
#if F_9DOF_GBY_KALMAN
            if (nine_axis_kalman)       scratch16 = (int16_t) (sfg->SV_9DOF_GBY_KALMAN.fDeltaPl * 100.0F);
#endif
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

            // [36-35]: fAccGl[CHX] resolution 1/8192 g
            // [38-37]: fAccGl[CHY] resolution 1/8192 g
            // [40-39]: fAccGl[CHZ] resolution 1/8192 g
            for (i = CHX; i <= CHZ; i++)
            {
                // default to zero data
                ftmp = 0.0F;
#if F_6DOF_GY_KALMAN
                if (six_axis_kalman)    ftmp = sfg->SV_6DOF_GY_KALMAN.fAccGl[i] * 8192.0F;
#endif
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   ftmp = sfg->SV_9DOF_GBY_KALMAN.fAccGl[i] * 8192.0F;
#endif

                // check for clipping
                if (ftmp > 32767.0F)            scratch16 = 32767;
                else if (ftmp < -32768.0F)      scratch16 = -32768;
                else                            scratch16 = (int16_t) ftmp;
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            }

            // [42-41]: fDisGl[CHX] resolution 0.01m
            // [44-43]: fDisGl[CHY] resolution 0.01m
            // [46-45]: fDisGl[CHZ] resolution 0.01m
            for (i = CHX; i <= CHZ; i++)
            {
                // default to zero data
                ftmp = 0.0F;
#if F_9DOF_GBY_KALMAN
                if (nine_axis_kalman)   ftmp = sfg->SV_9DOF_GBY_KALMAN.fDisGl[i] * 100.0F;
#endif

                // check for clipping
                if (ftmp > 32767.0F)            scratch16 = 32767;
                else if (ftmp < -32768.0F)      scratch16 = -32768;
                else                            scratch16 = (int16_t) ftmp;
                sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            }

            // [47]: add the tail byte for the Kalman packet type 7
            sUARTOutputBuffer[iIndex++] = 0x7E;
        }
    }   // end of check for Kalman packet
#endif
#if F_USING_ACCEL
    // *************************************************************************
    // fixed length packet type 8 transmitted whenever a precision accelerometer
    // measurement has been stored.
    // total size is 0 to 40 equals 41 bytes
    // *************************************************************************
    // check to see which packet (if any) is to be transmitted
    if (AccelCalPacketOn != -1)
    {
        // [0]: packet start byte (need a iIndex++ here since not using sBufAppendItem)
        sUARTOutputBuffer[iIndex++] = 0x7E;

        // [1]: packet type 8 byte (iIndex is automatically updated in sBufAppendItem)
        tmpuint8_t = 0x08;
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &tmpuint8_t, 1);

        // [2]: packet number byte
        sBufAppendItem(sUARTOutputBuffer, &iIndex, &iPacketNumber, 1);
        iPacketNumber++;

        // [3]: AccelCalPacketOn in range 0-11 denotes stored location and MAXORIENTATIONS denotes transmit
        // precision accelerometer calibration on power on before any measurements have been obtained.
        sBufAppendItem(sUARTOutputBuffer, &iIndex,
                       (uint8_t *) &(AccelCalPacketOn), 1);

        // [9-4]: stored accelerometer measurement fGs (scaled to 8192 counts per g)
        if ((AccelCalPacketOn >= 0) &&
            (AccelCalPacketOn < MAX_ACCEL_CAL_ORIENTATIONS))
        {
            scratch16 = (int16_t) (sfg->AccelBuffer.fGsStored[AccelCalPacketOn][CHX] * 8192.0F);
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            scratch16 = (int16_t) (sfg->AccelBuffer.fGsStored[AccelCalPacketOn][CHY] * 8192.0F);
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
            scratch16 = (int16_t) (sfg->AccelBuffer.fGsStored[AccelCalPacketOn][CHZ] * 8192.0F);
            sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        }
        else
        {
            // transmit zero bytes since this is the power on or reset transmission of the precision calibration
            sBufAppendZeros(sUARTOutputBuffer, &iIndex, 3);
        }

        // [15-10]: precision accelerometer offset vector fV (g scaled by 32768.0)
        scratch16 = (int16_t) (sfg->AccelCal.fV[CHX] * 32768.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fV[CHY] * 32768.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fV[CHZ] * 32768.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [21-16]: precision accelerometer inverse gain matrix diagonal finvW - 1.0 (scaled by 10000.0)
        scratch16 = (int16_t) ((sfg->AccelCal.finvW[CHX][CHX] - 1.0F) * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) ((sfg->AccelCal.finvW[CHY][CHY] - 1.0F) * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) ((sfg->AccelCal.finvW[CHZ][CHZ] - 1.0F) * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [27-22]: precision accelerometer inverse gain matrix off-diagonal finvW (scaled by 10000)
        scratch16 = (int16_t) (sfg->AccelCal.finvW[CHX][CHY] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.finvW[CHX][CHZ] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.finvW[CHY][CHZ] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [33-28]: precision accelerometer rotation matrix diagonal fR0 (scaled by 10000)
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHX][CHX] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHY][CHY] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHZ][CHZ] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [45-34]: precision accelerometer inverse rotation matrix off-diagonal fR0 (scaled by 10000)
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHX][CHY] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHX][CHZ] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHY][CHX] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHY][CHZ] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHZ][CHX] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);
        scratch16 = (int16_t) (sfg->AccelCal.fR0[CHZ][CHY] * 10000.0F);
        sBufAppendItem(sUARTOutputBuffer, &iIndex, (uint8_t *) &scratch16, 2);

        // [46]: add the tail byte for the packet type 8
        sUARTOutputBuffer[iIndex++] = 0x7E;

        // disable future packets of this type until a new measurement has been obtained
        sfg->pControlSubsystem->AccelCalPacketOn = -1;
    }
#endif  // F_USING_ACCEL
    // ********************************************************************************
    // all packets have now been constructed in the output buffer so now transmit.
    // The final iIndex++ gives the number of bytes to transmit which is one more than
    // the last index in the buffer. this function is non-blocking
    // ********************************************************************************
    sfg->pControlSubsystem->write(sfg->pControlSubsystem, sUARTOutputBuffer, iIndex);
    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

void DecodeCommandBytes	(	SensorFusionGlobals *	sfg,
		char	iCommandBuffer[],
		uint8	sUART_InputBuffer[],
		uint16	nbytes
	)

This function is responsible for decoding commands sent by the NXP Sensor Fusion Toolbox and setting the appropriate flags in the ControlSubsystem data structure. Packet protocols are defined in the NXP Sensor Fusion for Kinetis Product Development Kit User Guide.

Definition at line 90 of file DecodeCommandBytes.c.

Referenced by WIRED_UART_IRQHandler(), and WIRELESS_UART_IRQHandler().

{
    int32 isum;     // 32 bit command identifier
    int16 i, j;     // loop counters

    // parse all received bytes in sUARTInputBuf into the iCommandBuffer delay line
    for (i = 0; i < nbytes; i++) {
        // shuffle the iCommandBuffer delay line and add the new command byte
        for (j = 0; j < 3; j++)
            iCommandBuffer[j] = iCommandBuffer[j + 1];
        iCommandBuffer[3] = sUART_InputBuffer[i];

        // check if we have a valid command yet
        isum = ((((((int32)iCommandBuffer[0] << 8) | iCommandBuffer[1]) << 8) | iCommandBuffer[2]) << 8) | iCommandBuffer[3];
        switch (isum)       {
        case cmd_VGplus: // "VG+ " = enable angular velocity packet transmission
                    sfg->pControlSubsystem->AngularVelocityPacketOn = true;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_VGminus: // "VG- " = disable angular velocity packet transmission
                    sfg->pControlSubsystem->AngularVelocityPacketOn = false;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_DBplus: // "DB+ " = enable debug packet transmission
                    sfg->pControlSubsystem->DebugPacketOn = true;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_DBminus: // "DB- " = disable debug packet transmission
                    sfg->pControlSubsystem->DebugPacketOn = false;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_Q3: // "Q3  " = transmit 3-axis accelerometer quaternion in standard packet
#if F_3DOF_G_BASIC
                    sfg->pControlSubsystem->QuaternionPacketType = Q3;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_Q3M:  // "Q3M " = transmit 3-axis magnetometer quaternion in standard packet
#if F_3DOF_B_BASIC
                    sfg->pControlSubsystem->QuaternionPacketType = Q3M;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_Q3G: // "Q3G " = transmit 3-axis gyro quaternion in standard packet
#if F_3DOF_Y_BASIC
                    sfg->pControlSubsystem->QuaternionPacketType = Q3G;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_Q6MA: // "Q6MA" = transmit 6-axis mag/accel quaternion in standard packet
#if F_6DOF_GB_BASIC
                    sfg->pControlSubsystem->QuaternionPacketType = Q6MA;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_Q6AG: // "Q6AG" = transmit 6-axis accel/gyro quaternion in standard packet
#if F_6DOF_GY_KALMAN
                    sfg->pControlSubsystem->QuaternionPacketType = Q6AG;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_Q9: // "Q9  " = transmit 9-axis quaternion in standard packet (default)
#if F_9DOF_GBY_KALMAN
                    sfg->pControlSubsystem->QuaternionPacketType = Q9;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_RPCplus: // "RPC+" = Roll/Pitch/Compass on
                    sfg->pControlSubsystem->RPCPacketOn = true;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_RPCminus: // "RPC-" = Roll/Pitch/Compass off
                    sfg->pControlSubsystem->RPCPacketOn = false;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_ALTplus: // "ALT+" = Altitude packet on
                    sfg->pControlSubsystem->AltPacketOn = true;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_ALTminus: // "ALT-" = Altitude packet off
                    sfg->pControlSubsystem->AltPacketOn = false;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_RST: // "RST " = Soft reset
                    // reset sensor fusion
                    fInitializeFusion(sfg);

                    // reset magnetic calibration and magnetometer data buffer
#if F_USING_MAG
                    fInitializeMagCalibration(&sfg->MagCal, &sfg->MagBuffer);
#endif
                    // reset precision accelerometer calibration and accelerometer measurements
#if F_USING_ACCEL
                    fInitializeAccelCalibration(&sfg->AccelCal, &sfg->AccelBuffer, &(sfg->pControlSubsystem->AccelCalPacketOn)) ;
#endif
                    iCommandBuffer[3] = '~';
        break;

        case cmd_RINS: // "RINS" = Reset INS inertial navigation velocity and position
#if F_9DOF_GBY_KALMAN
                    for (i = CHX; i <= CHZ; i++) {
            sfg->SV_9DOF_GBY_KALMAN.fVelGl[i] = 0.0F;
            sfg->SV_9DOF_GBY_KALMAN.fDisGl[i] = 0.0F;
                    }
#endif
        iCommandBuffer[3] = '~';
        break;

        case cmd_SVAC: // "SVAC" = save all calibrations to Kinetis flash
                    SaveMagCalibrationToNVM(sfg);
                    SaveGyroCalibrationToNVM(sfg);
                    SaveAccelCalibrationToNVM(sfg);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_SVMC: // "SVMC" = save magnetic calibration to Kinetis flash
                    SaveMagCalibrationToNVM(sfg);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_SVYC: // "SVYC" = save gyroscope calibration to Kinetis flash
                    SaveGyroCalibrationToNVM(sfg);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_SVGC: // "SVGC" = save precision accelerometer calibration to Kinetis flash
                    SaveAccelCalibrationToNVM(sfg);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_ERAC: // "ERAC" = erase all calibrations from Kinetis flash
                    EraseMagCalibrationFromNVM();
                    EraseGyroCalibrationFromNVM();
                    EraseAccelCalibrationFromNVM();
                    iCommandBuffer[3] = '~';
        break;

        case cmd_ERMC: // "ERMC" = erase magnetic calibration offset 0 bytes from Kinetis flash
                    EraseMagCalibrationFromNVM();
                    iCommandBuffer[3] = '~';
        break;

        case cmd_ERYC: // "ERYC" = erase gyro offset calibrationoffset 128 bytes from Kinetis flash
                    EraseGyroCalibrationFromNVM();
                    iCommandBuffer[3] = '~';
        break;

        case cmd_ERGC: // "ERGC" = erase precision accelerometer calibration offset 192 bytesfrom Kinetis flash
                    EraseAccelCalibrationFromNVM();
                    iCommandBuffer[3] = '~';
        break;

        case cmd_180X: // "180X" perturbation
                    sfg->iPerturbation = 1;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_180Y: // "180Y" perturbation
                    sfg->iPerturbation = 2;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_180Z: // "180Z" perturbation
                    sfg->iPerturbation = 3;
                  iCommandBuffer[3] = '~';
        break;

        case cmd_M90X: // "M90X" perturbation
                    sfg->iPerturbation = 4;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_P90X: // "P90X" perturbation
                    sfg->iPerturbation = 5;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_M90Y: // "M90Y" perturbation
                    sfg->iPerturbation = 6;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_P90Y: // "P90Y" perturbation
                    sfg->iPerturbation = 7;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_M90Z: // "M90Z" perturbation
                    sfg->iPerturbation = 8;
                    iCommandBuffer[3] = '~';
        break;

        case cmd_P90Z: // "P90Z" perturbation
                    sfg->iPerturbation = 9;
                    iCommandBuffer[3] = '~';
        break;

#if F_USING_ACCEL
        case cmd_PA00: // "PA00" average precision accelerometer location 0
                    sfg->AccelBuffer.iStoreLocation = 0;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA01: // "PA01" average precision accelerometer location 1
                    sfg->AccelBuffer.iStoreLocation = 1;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA02: // "PA02" average precision accelerometer location 2
                    sfg->AccelBuffer.iStoreLocation = 2;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA03: // "PA03" average precision accelerometer location 3
                    sfg->AccelBuffer.iStoreLocation = 3;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA04: // "PA04" average precision accelerometer location 4
                    sfg->AccelBuffer.iStoreLocation = 4;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA05: // "PA05" average precision accelerometer location 5
                    sfg->AccelBuffer.iStoreLocation = 5;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA06: // "PA06" average precision accelerometer location 6
                    sfg->AccelBuffer.iStoreLocation = 6;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA07: // "PA07" average precision accelerometer location 7
                    sfg->AccelBuffer.iStoreLocation = 7;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA08: // "PA08" average precision accelerometer location 8
                    sfg->AccelBuffer.iStoreLocation = 8;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA09: // "PA09" average precision accelerometer location 9
                    sfg->AccelBuffer.iStoreLocation = 9;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA10: // "PA10" average precision accelerometer location 10
                    sfg->AccelBuffer.iStoreLocation = 10;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;

        case cmd_PA11: // "PA11" average precision accelerometer location 11
                    sfg->AccelBuffer.iStoreLocation = 11;
                    sfg->AccelBuffer.iStoreCounter = (ACCEL_CAL_AVERAGING_SECS * FUSION_HZ);
                    iCommandBuffer[3] = '~';
        break;
#endif // precision accelerometer calibration

        default:
            // no action
            break;
        }
    } // end of loop over received characters

    return;
}

Here is the call graph for this function:

Here is the caller graph for this function:

int8_t initializeControlPort

(

ControlSubsystem *

pComm

)

Call this once to initialize structures, ports, etc.

Call this once to initialize structures, ports, etc.

Parameters

pComm

pointer to the control subystem structure

Definition at line 182 of file control.c.

Referenced by main().

{
    uart_config_t   config;
    if (pComm)
    {
        pComm->DefaultQuaternionPacketType = Q3;    // default to simplest algorithm
        pComm->QuaternionPacketType = Q3;           // default to simplest algorithm
        pComm->AngularVelocityPacketOn = true;      // transmit angular velocity packet
        pComm->DebugPacketOn = true;                // transmit debug packet
        pComm->RPCPacketOn = true;                  // transmit roll, pitch, compass packet
        pComm->AltPacketOn = true;                 // Altitude packet
        pComm->AccelCalPacketOn = 0;
        pComm->write = writeControlPort;
        pComm->stream = CreateAndSendPackets;

#if F_USE_WIRED_UART
        /* Initialize WIRED UART pins below - currently duplicates code in pin_mux.c */
        CLOCK_EnableClock(WIRED_UART_PORT_CLKEN);
        PORT_SetPinMux(WIRED_UART_PORT, WIRED_UART_RX_PIN, WIRED_UART_MUX);
        PORT_SetPinMux(WIRED_UART_PORT, WIRED_UART_TX_PIN, WIRED_UART_MUX);
        UART_GetDefaultConfig(&config);

        config.baudRate_Bps = CONTROL_BAUDRATE;
        config.enableTx = true;
        config.enableRx = true;
        config.rxFifoWatermark = 1;
        UART_Init(WIRED_UART, &config, CLOCK_GetFreq(WIRED_UART_CLKSRC));

        /* Enable RX interrupt. */
        UART_EnableInterrupts(WIRED_UART, kUART_RxDataRegFullInterruptEnable |
                              kUART_RxOverrunInterruptEnable);
        EnableIRQ(WIRED_UART_IRQn);
#endif
#if F_USE_WIRELESS_UART
        /* Initialize WIRELESS UART pins below */
        CLOCK_EnableClock(WIRELESS_UART_PORT_CLKEN);
        PORT_SetPinMux(WIRELESS_UART_PORT, WIRELESS_UART_RX_PIN,
                       WIRELESS_UART_MUX);
        PORT_SetPinMux(WIRELESS_UART_PORT, WIRELESS_UART_TX_PIN,
                       WIRELESS_UART_MUX);

        UART_Init(WIRELESS_UART, &config, CLOCK_GetFreq(WIRELESS_UART_CLKSRC));
        BlueRadios_Init();

        /* Enable RX interrupt. */
        UART_EnableInterrupts(WIRELESS_UART, kUART_RxDataRegFullInterruptEnable |
                              kUART_RxOverrunInterruptEnable);
        EnableIRQ(WIRELESS_UART_IRQn);
#endif

        return (0);
    }
    else
    {
        return (1);
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

void sBufAppendItem	(	uint8_t *	pDest,
		uint16_t *	pIndex,
		uint8_t *	pSource,
		uint16_t	iBytesToCopy
	)

Utility function used to place data in output buffer about to be transmitted via UART.

Definition at line 59 of file output_stream.c.

Referenced by CreateAndSendPackets(), and sBufAppendZeros().

{
    uint16_t    i;  // loop counter

    // loop over number of bytes to add to the destination buffer
    for (i = 0; i < iBytesToCopy; i++)
    {
        switch (pSource[i])
        {
            case 0x7E:
                // special case 1: replace 0x7E (start and end byte) with 0x7D and 0x5E
                pDest[(*pIndex)++] = 0x7D;
                pDest[(*pIndex)++] = 0x5E;
                break;

            case 0x7D:
                // special case 2: replace 0x7D with 0x7D and 0x5D
                pDest[(*pIndex)++] = 0x7D;
                pDest[(*pIndex)++] = 0x5D;
                break;

            default:
                // general case, simply add this byte without change
                pDest[(*pIndex)++] = pSource[i];
                break;
        }
    }

    return;
}

Here is the caller graph for this function:

Variable Documentation

uint8_t sUARTOutputBuffer[256]

main output buffer defined in control.c

Definition at line 59 of file control.c.